#ifndef __SUBMAP_H_
#define __SUBMAP_H_

#include <slam_2d_ros/common/slam_define.h>
#include <slam_2d_ros/common/frame.h>
#include <slam_2d_ros/icp_2d/likelihood_filed.h>
#include <slam_2d_ros/mapping/occupancy_map.h>

namespace slam_2d{

class Submap{
    public:
        // 构造函数 
        Submap(const Pose2D& pose);

        // 在子地图中增加一个关键帧
        void AddKeyFrame(boost::shared_ptr<Frame> frame);
        // 设置位姿
        void SetPose(const Pose2D& pose);
        // 设置子图ID
        void SetId(size_t id);
        // 将frame与本submap进行匹配，计算frame->pose
        bool MatchScan(boost::shared_ptr<Frame> frame);
        // 在栅格地图中增加一个帧
        void AddScanInOccupancyMap(boost::shared_ptr<Frame> frame);
        // 当子地图的位姿更新时，重设每个frame的世界位姿
        void UpdateFramePoseWorld();
        // 判定当前的scan是否有位于submap外部的点
        bool HasOutsidePoints() const;
        // 把另一个submap中的占据栅格复制到本地图中
        void SetOccuFromOtherSubmap(boost::shared_ptr<Submap> other);

        // 获取子图ID
        size_t GetId() const { 
            return id_; 
        }
        // 获取子图位姿
        Pose2D GetPose() const { 
            return pose_; 
        }
        // 获取子图中的关键帧数量
        size_t NumFrames() const { 
            return frames_.size(); 
        }
        // 获取子图中的关键帧
        std::vector<boost::shared_ptr<Frame>>& GetFrames() { 
            return frames_; 
        }
        // 获取占用地图
        OccupancyMap& GetOccuMap() { 
            return occu_map_; 
        }
        // 获取likelihood field
        LikelihoodField& GetLikelihood() { 
            return field_; 
        }

    private:
        Pose2D pose_;       //子图位姿
        size_t id_ = 0;     //子图ID

        std::vector<boost::shared_ptr<Frame>> frames_;    // 一个submap中的关键帧
        LikelihoodField field_;                       // 用于匹配
        OccupancyMap occu_map_;                       // 用于生成栅格地图

};

} // namespace slam_2d

#endif